import raspi_message
from pyb import Timer, Pin, UART
uart = UART(4, 9600)
from machine import SoftI2C
from motor import Motors
from pyb import Pin,delay
from FBLSensor import GoCheckFBLSensor,BackCheckFBLSensor,FrontBackCheckRun1,SlowStop
# 多功能电机模块I2C端口
i2c1=SoftI2C(sda=Pin("B11"), scl=Pin("B10"))
motors=Motors(i2c1)
mode = '1'  # 1: 表示蓝牙模式 2: 循迹模式
def Go(Speed):  # ++++ 前进
    Speed=int(Speed*40.95)
    motors.speed(0,Speed) #直流电机0正转，速度0~4095,4095表示最快速度
    motors.speed(1,Speed)
    motors.speed(2,Speed)
    motors.speed(3,Speed)
    print('Go')

def Back(Speed):# ---- 后退
    Speed=int(Speed*40.95)
    motors.speed(0,-Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,-Speed)
    motors.speed(2,-Speed)
    motors.speed(3,-Speed)
    print('Back')

def LeftGo(Speed): # +-+- 左走
    Speed=int(Speed*40.95)
    motors.speed(0,Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,-Speed)
    motors.speed(2,Speed)
    motors.speed(3,-Speed)
    print('LeftGo')

def RightGo(Speed):  # -+-+ 右走
    Speed=int(Speed*40.95)
    motors.speed(0,-Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,Speed)
    motors.speed(2,-Speed)
    motors.speed(3,Speed)
    print('RightGO')

def TurnRight(Speed):  # -++- 左转
    Speed=int(Speed*40.95)
    motors.speed(0,-Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,Speed)
    motors.speed(2,Speed)
    motors.speed(3,-Speed)
    print('TurnRight')

def TurnLeft(Speed): # +--+ 右转
    Speed=int(Speed*40.95)
    motors.speed(0,Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,-Speed)
    motors.speed(2,-Speed)
    motors.speed(3,Speed)
    print('TurnLeft')

def LeftGo45(Speed):  # +0+0 左前45°
    Speed=int(Speed*40.95)
    motors.speed(0,Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,0)
    motors.speed(2,Speed)
    motors.speed(3,0)
    print('LeftGo45')

def RightGo45(Speed):  # 0+0+ 右前45°
    Speed=int(Speed*40.95)
    motors.speed(0,0) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,Speed)
    motors.speed(2,0)
    motors.speed(3,Speed)
    print('RightGo45')

def LeftBack45(Speed):  # 0-0- 左后45°
    Speed=int(Speed*40.95)
    motors.speed(0,0) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,-Speed)
    motors.speed(2,0)
    motors.speed(3,-Speed)
    print('LeftBack45')

def RightBack45(Speed): # -0-0 右后45°
    Speed=int(Speed*40.95)
    motors.speed(0,-Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,0)
    motors.speed(2,-Speed)
    motors.speed(3,0)
    print('RightBack45')

def HardStop():
    #制动停止
    for i in range(4):
        motors.brake(i)
    print('HardStop')

def SoftStop():
    motors.speed(0,0) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,0)
    motors.speed(2,0)
    motors.speed(3,0)
    print('SoftStop')

def bluetooth(BLE):
    #global mode
    print('lanya')
    while 1:
        if BLE.uart.any():  # 查询是否有信息
            text = BLE.uart.read(1)  # 默认单次最多接收128字节
            if text.find(b'o')>-1:#停机
                HardStop()
                mode='3'
                return 3
            elif text.find(b's')>-1:
                HardStop()
                mode="2"#循迹
                return 2
            elif text.find(b'c')>-1:
                HardStop()
            #向前
            elif text.find(b'g')>-1:
                Go(100)
            #后退
            elif text.find(b'b') >-1:
                Back(100)
            #左走
            elif text.find( b'l')>-1:
                LeftGo(100)
            #右走
            elif text.find( b'r')>-1:
                RightGo(100)
            #左转
            elif text.find( b'm')>-1:
                TurnLeft(100)
            #右转
            elif text.find(b'n')>-1:
                TurnRight(100)
            #左前45
            elif text.find(b'h')>-1:
                LeftGo45(100)
            #右前45
            elif text.find( b'i')>-1:
                RightGo45(100)
            #左后45
            elif text.find( b'j')>-1:
                LeftBack45(100)
            #右后45
            elif text.find( b'k')>-1:
                RightBack45(100)
def HowRun(lrg):
    if lrg=='g':
        print('Go')
        Go(TrackSpeed)
        delay(50)
    elif lrg=='l':
        print('LeftGo')
        LeftGo(TrackSpeed)
        delay(30)
    elif lrg=='m':
        print('TurnLeft')
        TurnLeft(TrackSpeed)
        delay(50)
    elif lrg=='r':
        print('RightGo')
        RightGo(TrackSpeed)
        delay(30)
    elif lrg=='n':
        print('TurnRight')
        TurnRight(TrackSpeed)
        delay(50)
    else:
        Go(TrackSpeed)
        delay(50)
        print('ElseGo')
TrackSpeed=80
def go_ahead():
    while 1:
        if uart.any():
            return(0)
        frontbackleft=GoCheckFBLSensor()
        lrg=FrontBackCheckRun1(frontbackleft[0],frontbackleft[1])
        ss=SlowStop(frontbackleft[2])#根据左面循迹模块判断是否停下或减速
        print(ss)
        if ss=='s':
            print('Slow')
            TrackSpeed=60
            HowRun(lrg)
            delay(50)
        elif ss=='f':
            print('Fast')
            TrackSpeed=80
            HowRun(lrg)
            delay(50)
        elif ss=='c':
            print('Stop')
            HardStop()
            uart.write('stop')
            if uart.read(32) == 'stop':
                return 1
            delay(50)

def go_back():

def go_ahead_fast():
    while 1:
        if uart.any():
            return(0)
        frontbackleft=GoCheckFBLSensor()
        lrg=FrontBackCheckRun1(frontbackleft[0],frontbackleft[1])
        ss=SlowStop(frontbackleft[2])#根据左面循迹模块判断是否停下或减速
        print(ss)
        if ss=='s':
            print('Slow')
            TrackSpeed=60
            HowRun(lrg)
            delay(50)
        elif ss=='f':
            print('Fast')
            TrackSpeed=80
            HowRun(lrg)
            delay(50)
        elif ss=='c':
            print('locationchanged')
            uart.write('locationchanged')
            if uart.read(32) == 'stop':
                return 1
            delay(50)



def go_back_fast():
    if uart.any():
        text = uart.read(32)
        reply = text
        uart.write(reply)
        if text == 'go_back  ,f':
            go_back()

def takeing_box():

def laydown_box():

def stop():


def main():
    while 1:
        text = raspi_message.receive_text()
        if text == 'go_ahead':
            go_ahead()
        elif text == 'go_back':
            go_back()
        elif text == 'go_ahead_fast':
            go_ahead_fast()
        elif text == 'go_back_fast':
            go_back_fast()
        elif text == 'takeing_box':
            takeing_box()
        elif text == 'laydown_box':
            laydown_box()
        elif text == 'stop':
            stop()
        else:
            pass
